import tkinter as tk
from tkinter import ttk
import serial
import struct
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
import threading
import time
import json

# 串口设置
# ser = serial.Serial('COM5', 115200)  # 替换为您的串口
data_list = []  # 用于存储数据的列表
base_location = []  # 基站位置列表

def parse_data_packet(packet_data):
    packet_data = packet_data[:-2]
    unpacked_data = struct.unpack('<4B 10H 2B', packet_data)
    distances = [unpacked_data[5] / 100, unpacked_data[6] / 100, unpacked_data[7] / 100]
    loc = locate_tag_numerical(base_location, distances)

    return {
        'tag_address': unpacked_data[3],
        'frame_num': unpacked_data[4],
        'distance1': unpacked_data[5],
        'distance2': unpacked_data[6],
        'distance3': unpacked_data[7],
        'accel_x': unpacked_data[8] / 100.0,
        'accel_y': unpacked_data[9] / 100.0,
        'accel_z': unpacked_data[10] / 100.0,
        'roll': unpacked_data[11] / 100.0,
        'pitch': unpacked_data[12] / 100.0,
        'yaw': unpacked_data[13] / 100.0,
        'position': loc
    }

def locate_tag_numerical(base_stations, distances):
    def residuals(p, base_stations, distances):
        x, y = p
        res = []
        for (bx, by), d in zip(base_stations, distances):
            res.append(np.sqrt((x - bx) ** 2 + (y - by) ** 2) - d)
        return res

    from scipy.optimize import least_squares
    initial_guess = [0, 0]
    result = least_squares(residuals, initial_guess, args=(base_stations, distances))
    return [(result.x[0], result.x[1])] if result.success else []

def write_data_to_json(data):
    with open('data.json', 'a') as f:
        json.dump(data, f)
        f.write('\n')  # 每行一个 JSON 对象

def read_serial():
    while True:
        if ser.in_waiting > 0:
            packet_data = ser.read(30)  # 根据实际数据包大小调整
            result = parse_data_packet(packet_data)
            data_list.append(result)
            write_data_to_json(result)  # 写入 JSON 文件
            update_gui(result)
        time.sleep(0.1)

def update_gui(data):
    position = data['position'][0] if data['position'] else (0, 0)
    label_position.config(text=f"位置: X={position[0]:.2f}, Y={position[1]:.2f}")

    # 更新图形
    x_data.append(position[0])
    y_data.append(position[1])
    accel_x_data.append(data['accel_x'])
    accel_y_data.append(data['accel_y'])
    accel_z_data.append(data['accel_z'])

    ax.clear()
    ax.scatter(*zip(*base_location), color='blue', s=100, label='基站')
    ax.plot(x_data, y_data, 'ro-', label='标签轨迹')
    ax.set_title('标签位置轨迹', fontsize=14)
    ax.set_xlabel('X 位置')
    ax.set_ylabel('Y 位置')
    ax.legend()

    # 绘制加速度信息
    ax_accel.clear()
    ax_accel.plot(accel_x_data, label='加速度 X', color='r')
    ax_accel.plot(accel_y_data, label='加速度 Y', color='g')
    ax_accel.plot(accel_z_data, label='加速度 Z', color='b')
    ax_accel.set_title('加速度信息', fontsize=14)
    ax_accel.set_xlabel('时间')
    ax_accel.set_ylabel('加速度')
    ax_accel.legend()

    canvas.draw()

def add_base_station():
    try:
        x = float(entry_x.get())
        y = float(entry_y.get())
        base_location.append((x, y))
        entry_x.delete(0, tk.END)
        entry_y.delete(0, tk.END)
        update_plot()
    except ValueError:
        print("请输入有效的坐标")

def update_plot():
    ax.clear()
    if base_location:
        ax.scatter(*zip(*base_location), color='blue', s=100, label='基站')
    ax.set_title('标签位置轨迹', fontsize=14)
    ax.set_xlabel('X 位置')
    ax.set_ylabel('Y 位置')
    ax.legend()
    canvas.draw()

# 创建主窗口
root = tk.Tk()
root.title("标签位置追踪")

# 添加顶部Logo
logo_image = tk.PhotoImage(file="logo.png")
logo_label = tk.Label(root, image=logo_image)
logo_label.pack(pady=10)

# 创建标签用于显示位置
label_position = ttk.Label(root, text="位置: X=0.00, Y=0.00", font=("Arial", 14), foreground="black")
label_position.pack(pady=10)

# 创建基站坐标输入框
frame_input = ttk.Frame(root)
frame_input.pack(pady=10)

ttk.Label(frame_input, text="基站 X:").grid(row=0, column=0)
entry_x = ttk.Entry(frame_input)
entry_x.grid(row=0, column=1)

ttk.Label(frame_input, text="基站 Y:").grid(row=1, column=0)
entry_y = ttk.Entry(frame_input)
entry_y.grid(row=1, column=1)

button_add = ttk.Button(frame_input, text="添加基站", command=add_base_station)
button_add.grid(row=2, columnspan=2, pady=5)

# 创建 matplotlib 图形
fig, (ax, ax_accel) = plt.subplots(2, 1, figsize=(8, 8))
canvas = FigureCanvasTkAgg(fig, master=root)
canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True, padx=10, pady=10)

# 存储位置和加速度数据
x_data = []
y_data = []
accel_x_data = []
accel_y_data = []
accel_z_data = []

# 启动串口读取线程
threading.Thread(target=read_serial, daemon=True).start()

# 启动主循环
root.mainloop()
